#ifndef FHE_MATH_ROT_H
#define FHE_MATH_ROT_H

#include <fhe/math/Math.h>
#include <fhe/core/FileSystem.h>
#include <string>
#include <sstream>
#include <iomanip>
#include <typeinfo>

namespace fhe
{
  namespace core
  {
    template <typename T>
    class Rot<2,T>
    {
    public:
      Rot()
        : a_( 0 )
      {
      }
      
      static Rot fromRadians( double radians )
      {
        return Rot( radians );
      }
      
      static Rot fromDegrees( double degrees )
      {
        return Math::radians( degrees );
      }
      
      double radians() const
      {
        return a_;
      }
      
      double degrees() const
      {
        return Math::degrees( a_ );
      }
      
      bool operator==( const Rot& r ) const
      {
        return Math::equal( a_, r.a_ );
      }
      
      bool operator!=( const Rot& r ) const
      {
        return !operator==(r);
      }
      
      Rot inverse() const
      {
        return Rot( -a_ );
      }
      
      Rot operator+( const Rot& r ) const
      {
        return Rot( a_ + r.a_ );
      }
      
      Rot operator-( const Rot& r ) const
      {
        return Rot( a_ - r.a_ );
      }
      
      static std::string typeName()
      {
        return std::string( "Rot2" ) + typeid(T).name();
      }
      
      std::string toString() const
      {
        std::ostringstream os;
        os << typeName() << "(" << degrees() << ")";
        return os.str();
      }
      
      static void serialize( const Data::Ptr& data, Rot r )
      {
        FileSystem::instance().serialize( data->addChild( "radians" ), r.radians() );
      }
      
      static Rot deserialize( const Data::Ptr& data )
      {
        if ( data->hasChild( "degrees" ) )
        {
          return fromDegrees( FileSystem::instance().deserialize<T>( data->getChild( "degrees" ) ) );
        }
        if ( data->hasChild( "radians" ) )
        {
          return fromRadians( FileSystem::instance().deserialize<T>( data->getChild( "radians" ) ) );
        }
        else
        {
          return IDENTITY;
        }
      }
      
      static const Rot IDENTITY;
      
    private:
      Rot( double a )
        : a_( a )
      {
      }
      
      double a_;
    };
    
    template <typename T>
    const Rot<2,T> Rot<2,T>::IDENTITY = Rot<2,T>::fromRadians( 0 );
    
    template <typename T>
    class Rot<3,T>
    {
    public:
      typedef Vec<3,T> V;
      
      T w, x, y, z;
      
      Rot()
        : w( 1 )
        , x( 0 )
        , y( 0 )
        , z( 0 )
      {
      }
      
      Rot( T _w, T _x, T _y, T _z )
        : w( _w )
        , x( _x )
        , y( _y )
        , z( _z )
      {
      }
      
      Rot( const V& axis, T angle )
      {
        V axisNorm = axis.normalize();
        T sa = Math::sin( angle / 2. );
        T ca = Math::cos( angle / 2. );
        operator=( Rot( ca, axisNorm.x * sa, axisNorm.y * sa, axisNorm.z * sa ) );
      }
      
      void toAxisAngle( V& axis, T& angle ) const
      {
        Rot q = normalize();
        T ca = q.w;
        angle = Math::acos( ca ) * 2.;
        T sa = Math::sqrt( 1. - ca * ca );
        if ( Math::abs( sa ) < Math::EPS )
        {
          sa = 1;
        }
        axis = V( q.x / sa, q.y / sa, q.z / sa );
      }
      
      bool operator==( const Rot& r ) const
      {
        V axis1, axis2;
        T angle1, angle2;
        toAxisAngle( axis1, angle1 );
        r.toAxisAngle( axis2, angle2 );
        return ( axis1 == axis2 && Math::equal( angle1, angle2 ) ) ||
               ( axis1 == -axis2 && Math::equal( angle1, -angle2 ) );
      }
      
      bool operator!=( const Rot& r ) const
      {
        return !operator==( r );
      }
      
      Rot operator*( const Rot& r ) const
      {
        return Rot(w * r.w - x * r.x - y * r.y - z * r.z,
                   w * r.x + x * r.w + y * r.z - z * r.y,
                   w * r.y + y * r.w + z * r.x - x * r.z,
                   w * r.z + z * r.w + x * r.y - y * r.x);
      }
      
      V operator*( const V& v ) const
      {
        Rot r = *this * Rot( 0, v.x, v.y, v.z ) * inverse();
        return V( r.x, r.y, r.z );
      }
      
      Rot operator*( T d ) const
      {
        return Rot( w * d, x * d, y * d, z * d );
      }
      
      Rot operator/( T d ) const
      {
        return Rot( w / d, x / d, y / d, z / d );
      }
      
      Rot conjugate() const
      {
        return Rot( w, -x, -y, -z );
      }
      
      T magnitude() const
      {
        return Math::sqrt( w * w + x * x + y * y + z * z );
      }
      
      Rot normalize() const
      {
        return operator/( magnitude() );
      }
      
      Rot inverse() const
      {
        return normalize().conjugate();
      }
      
      static std::string typeName()
      {
        return std::string( "Rot3" ) + typeid(T).name();
      }
      
      std::string toString() const
      {
        V axis;
        T angle;
        toAxisAngle( axis, angle );
        std::ostringstream os;
        os << typeName() << "(" << axis << ", " << angle << ")";
        return os.str();
      }
      
      static void serialize( const Data::Ptr& data, Rot r )
      {
        V axis;
        T angle;
        r.toAxisAngle( axis, angle );
        FileSystem::instance().serialize( data->addChild( "axis" ), axis );
        FileSystem::instance().serialize( data->addChild( "angle" ),
                                          Rot<2,T>::fromRadians( angle ) );
      }
      
      static Rot deserialize( const Data::Ptr& data )
      {
        return Rot( data->hasChild( "axis" ) ? 
                      V::deserialize( data->getChild( "axis" ) ) :
                      V::ONE,
                    data->hasChild( "angle" ) ?
                      Rot<2,T>::deserialize( data->getChild( "angle" ) ).radians() :
                      0 );
      }
      
      static const Rot IDENTITY;
    };
    
    template <typename T>
    const Rot<3,T> Rot<3,T>::IDENTITY;

    template <size_t D, typename T>
    std::ostream& operator<<( std::ostream& os, const Rot<D,T>& r )
    {
      return os << r.toString();
    }
    
    typedef Rot<2,double> Rot2d;
    typedef Rot<2,int> Rot2i;
    typedef Rot<3,double> Rot3d;
    typedef Rot<3,int> Rot3i;
  }
}

#endif
